#ifndef _MAP_NODE_H_
#define _MAP_NODE_H_

#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>
#include "roboard_drivers/imu.h"
#include "saint_nr_9/IRdata.h"
#include "saint_nr_9/RobotPosition.h"
#include "PositionInformation.h"
#include <string.h>
#include "MotorControl.h"
#include "StateMachine.h"
#include <stdio.h>
#include <vector>
#include <algorithm>
#include <climits>
#include <stdexcept>
#include <cmath>
#include <fstream>
#include "queen.h"

// For the values in the internal map matrix
#define MAP_UNEXPLORED  0
#define MAP_FREE        1
#define MAP_WALL        2


// Max distance to matter for left and right wall
#define LEFT_WALL_FAR   30
#define RIGHT_WALL_FAR  30

#define HALF_ROBO_WIDTH 10

#define ROWS 1000
#define COLS 1000

using namespace roboard_drivers;

using std::ofstream;
using std::string;
using std::endl;
using std::min;
using std::max;
using std::cout;
using std::stringstream;



typedef unsigned char map_type;
map_type **mat;

struct position
{
  position() { x = 0; y = 0; }
  position(int ix, int iy) { x = ix; y = iy; }
  position(const position& pos) { x = pos.x; y = pos.y; }
  position& operator=(const position& pos) { x = pos.x; y = pos.y; return *this; }
  int x;
  int y;
};

int r = 0;
int l = 0;
int global_state = STRAIGHT_WALL;

position robo_pos(STARTPOSITION_X, STARTPOSITION_Y);
position prev_robo_pos(STARTPOSITION_X, STARTPOSITION_Y);

int sl = l;
int sr = r;
bool encoder_subscription_started = false;

saint_nr_9::IRdata ir_data;
saint_nr_9::RobotPosition robo_pos_msg;

ros::Publisher map_pub;
ros::Publisher vis_pub;
ros::Publisher rob_pos_pub;
ros::Publisher pos_pub;

std::vector<position> tag_list;

direction move = down;

bool new_right_wall = true;
double right_wall_dist = 0.0;
double left_wall_dist = 0.0;
direction wall_dir = left;
position current_wall_pos;

position pos_right_front;
position pos_right_back;
position pos_left_front;
position pos_left_back;

int right_front, right_back, left_front, left_back;
bool right_front_far, right_back_far, left_front_far, left_back_far;

position start_right_wall;
position end_right_wall;

std::vector<position> left_wall_points;
std::vector<position> left_wall_points_robo;

void state_update(const std_msgs::Int32::ConstPtr& state);
void recive_encoder(const Encoder::ConstPtr &msg);
void recieve_ir(const saint_nr_9::IRdata::ConstPtr &data);

bool equal_direction(direction one, direction two);
direction calculate_direction(direction old_direction, bool turn_right);
void publish_grid();

void create_marker(int x, int y, int id);
void send_position();
void straight_odo_calc(bool forward);
void draw_straight_line(const position& start, const position& end, map_type type, int thickness);
void fill_area(const position& pos1, const position& pos2, map_type value, bool overwrite_walls);
int median(const std::vector<int>& v);
void build_wall_segments(const std::vector<position>& left_wall_points, const std::vector<position>& robot_points, const direction& move);
void wall_adjust(bool following_right);
void calc_and_paint_walls_and_free_space();
void sensor_calc();
bool find_closest_wall(const position& pos, const direction& dir, position& wall_pos, int& distance);
void save_map(string path);
void save_map_real(string path);
void readMap(string path);
void initialize_map(const std_msgs::String::ConstPtr& signal);
#endif
